#ifndef RBTCTRLLER_H
#define RBTCTRLLER_H

#include <Eigen/Dense>
#include"IIRfilter.hpp"
#include"CtrlConstant.hpp"

namespace ctrl{

// signal input, output, filter coeficient all in double type
class rbt_ctrller
{
public:
    int control_type = 0;   
    std::vector<IIR_filt> doubleloop_pc;
    std::vector<IIR_filt> doubleloop_vc;
public:

    rbt_ctrller(){}
    ~rbt_ctrller(){}

    void Config(int robot_type, int control_type_in){
        control_type = control_type_in;
        if (control_type == DOUBLELOOPCTRL){
            doubleloop_pc.resize(robot_type);
            doubleloop_vc.resize(robot_type);
        }
    }

    /**
     * @brief tunning the controller parameter, need to generate para by matlab script
     * notice nume, deno is digital
     */
    void eztunning(int axis_id, Eigen::VectorXd pnume, Eigen::VectorXd pdeno, Eigen::VectorXd vnume, Eigen::VectorXd vdeno){
        doubleloop_pc[axis_id].set_coeffs(pnume, pdeno);
        doubleloop_vc[axis_id].set_coeffs(vnume, vdeno);
    }

};

}

#endif